/***********************************************************
    initial_alignment.h
    created on 7 June 2021 by dzXia Whu.sgg
    last modified: 06/09
***********************************************************/

#ifndef INITIAL_ALIGNMENT_
#define INITIAL_ALIGNMENT_

#include "..\Eigen\Dense"

//const variable
//unused, use macros instead. if used, need to add definition in .cpp
//extern const double kPi;    // PI
//extern const kLat;          // local latitude
//extern const double kOmega; // rotational angular velocity of the earth
//extern const double kG;     // gravity acceleration

#define OMEGA 7.292115E-5   // rotational angular velocity of the earth
//#define GRAVITY 9.7936174   // gravity acceleration
//#define LAT (30.5 / 180 * PI)   //local latitude, units:rad

// initial alignment class which uses the analysis coarse method.
// Example:
//   MatrixXd raw_accel;	// known accel and gyro data for initial alignment
//   MatrixXd raw_gyro;	    // row: epoch_num, col:3
//   InitAlign init_align;
//   init_align.set(raw_accel, raw_gyro);
//   Matrix3d init_rot_mat;	// C_b^n, init rotation matrix from sensor frame to nav frame obtained by init-alignment
//   init_rot_mat = init_align.initAlign();	//obtain rot-mat by invoking function initAlign()  
// Caution:
//   @parameters [in]  raw_accel,raw_gyro,whose size is (epoch_num,3),type is Eigen::MatrixXd
//   @parameters [out] init_rot_mat, the rot matrix obtained by init-align,whose type is Eigen::Matrix3d
class InitAlign {
public: //function
    /*******************************************************
    * set accel and gyro data by using static alignment raw data
    * @paras  gravity(m/s^2) and lat(units:rad)
    * @return void
    *******************************************************/
    void set(const Eigen::MatrixXd& accel, const Eigen::MatrixXd& gyro,double gravity,double lat);

    /*******************************************************
    * use analysis method to <coarse init-align>
    * @paras
    * @return rotation matrix C_b^n, means from the sensor frame to nav frame
    *******************************************************/
    Eigen::Matrix3d initAlign();
private: 
    Eigen::MatrixXd _accel; // static raw accel&gyro data, use for analysis coarse alignment 
    Eigen::MatrixXd _gyro;  // should be MatrixXd(n,3)
    double _gravity;        // gravity acceleration
    double _lat;            // local latitude, units:rad
};

#endif INITIAL_ALIGNMENT_ // INITIAL_ALIGNMENT_


/**
 * @brief  rotation matrix to euler angle by Eigen
 * @note
 * @param  [in] RotationMatrix type rot_mat
 * @retval Euler
 */
Eigen::Vector3d RotationMatrixToEuler(const Eigen::Matrix3d & rot_mat);


/**
 * @brief  euler angle to quaternion by Eigen
 * @note
 * @param  [in] Euler type euler
 * @retval Quaternion
 */
Eigen::Quaterniond EulerToQuaternion(const Eigen::Vector3d& euler);
